﻿
// 2022/6/15: 首个版本
// 2022/6/16: 增加BagFileReader
// 2022/7/5: 修正writeToBagFile
// 2022/11/11: 增加writeToBagFile和BagFileReader::initialize返回值
// 2023/10/9: 添加英文注释
// 2024/6/12: 支持spadas9

#ifndef ROS_SUPPORT_H
#define ROS_SUPPORT_H

#include "ros_support_base.h"
#include <ros/message_traits.h>
#include <ros/serialization.h>

// Need to import library / 需要添加库依赖: roscpp_serialization

namespace spadas
{
    /// \~English @brief Write ROS data object to the bag file
    /// \~Chinese @brief 将ROS数据对象写入bag文件
    /// \~English @param caller Cross-module function caller interface
    /// \~Chinese @param caller 跨模块函数调用接口
    /// \~English @param rawFolder Whether to write to the bag file in the raw data folder (input), otherwise it is the generation data folder (output)
    /// \~Chinese @param rawFolder 是否写至原始数据文件夹(input)内的bag文件，否则为generation数据文件夹(output)
    /// \~English @param fileName File name (do not need to include the suffix ".bag")
    /// \~Chinese @param fileName 文件名(不需要包括后缀.bag)
    /// \~English @param topic Topic
    /// \~Chinese @param topic Topic
    /// \~English @param time The time offset, in seconds
    /// \~Chinese @param time 相对时间戳，单位秒
    /// \~English @param obj ROS data object
    /// \~Chinese @param obj ROS数据对象
    /// \~English @returns Whether the data is successfully written
    /// \~Chinese @returns 是否成功写入
    template<typename Type>
    Bool writeToBagFile(Interface<ICrossCaller> caller, Bool rawFolder, String fileName, String topic, Double time, Type& obj)
    {
        uint32_t binaryDataLength = ros::serialization::serializationLength(obj);
        Binary binaryData(binaryDataLength);
        ros::serialization::OStream s(binaryData.data(), binaryDataLength);
        ros::serialization::serialize(s, obj);

        RosFileIOWriteMessageObject ioObject;
        ioObject.setWriteToGeneration(!rawFolder);
        ioObject.setFileName(fileName);
        ioObject.setTopic(topic);
        ioObject.setTime(time);
        ioObject.setDataType(ros::message_traits::datatype(obj));
        ioObject.setMD5Sum(ros::message_traits::md5sum(obj));
        ioObject.setDefinition(ros::message_traits::definition(obj));
        ioObject.setBinaryData(binaryData);
        return caller->callNativeFunction("RosFileIO", "WriteMessage", ioObject.as<BaseObject>());
    }

    /// \~English @brief One ROS data frame
    /// \~Chinese @brief 一帧ROS数据
    template<typename Type>
    struct BagObject
    {
        /// \~English @brief The time offset, in seconds
        /// \~Chinese @brief 相对时间戳，单位秒
        Double time;

        /// \~English @brief ROS data object
        /// \~Chinese @brief ROS数据对象
        Type obj;
    };

    /// \~English @brief Read ROS data from the bag file
    /// \~Chinese @brief 从bag文件读取ROS数据
    template<typename Type>
    class BagFileReader
    {
    public:
        /// \~English @brief Initialization
        /// \~Chinese @brief 初始化
        /// \~English @param caller Cross-module function caller interface
        /// \~Chinese @param caller 跨模块函数调用接口
        /// \~English @param rawFolder Whether to read data from bag file in raw data folder (input), otherwise it is generation data folder (output)
        /// \~Chinese @param rawFolder 是否从原始数据文件夹(input)内的bag文件读取数据，否则为generation数据文件夹(output)
        /// \~English @param fileName File name (do not need to include the suffix ".bag")
        /// \~Chinese @param fileName 文件名(不需要包括后缀.bag)
        /// \~English @param topic Topic
        /// \~Chinese @param topic Topic
        /// \~English @param streamSize Size of the buffer queue
        /// \~Chinese @param streamSize 缓存队列的大小
        /// \~English @returns Whether the initialization is successful
        /// \~Chinese @returns 是否成功初始化
        Bool initialize(Interface<ICrossCaller> caller, Bool rawFolder, String fileName, String topic, UInt streamSize)
        {
            stream = Stream<RosFileIOReadMessage>(math::max(1u, streamSize));

            RosFileIORegisterReadStreamObject ioObject;
            ioObject.setReadFromGeneration(!rawFolder);
            ioObject.setFileName(fileName);
            ioObject.setTopic(topic);
            ioObject.setReadStream(stream);

            if (!caller->callNativeFunction("RosFileIO", "RegisterReadStream", ioObject.as<BaseObject>()))
            {
                stream.terminate();
                return FALSE;
            }
            else return TRUE;
        }

        /// \~English @brief Retrieves all new data from the buffer queue
        /// \~Chinese @brief 从缓存队列中取出所有新数据
        Array<BagObject<Type> > dequeue()
        {
            auto newMessages = stream.dequeue();
            if (newMessages.isEmpty()) return Array<BagObject<Type> >();

            Array<BagObject<Type> > output = Array<BagObject<Type> >::createUninitialized(newMessages.size());
            for (UInt i = 0; i < newMessages.size(); i++)
            {
                RosFileIOReadMessage msg = newMessages[i];
                BagObject<Type> newObject;
                newObject.time = msg.time;
                ros::serialization::IStream s(msg.binaryData.data(), msg.binaryData.size());
                ros::serialization::deserialize(s, newObject.obj);
                output.initialize(i, newObject);
            }
            return output;
        }

        /// \~English @brief Whether it has been read to the end
        /// \~Chinese @brief 是否已读至末尾
        Bool endOfFile()
        {
            return stream.isTerminated();
        }

    private:
        Stream<RosFileIOReadMessage> stream;
    };
}

#endif